Lab 2 - Struktury danych w planowaniu ruchu

Logotyp PP

Metody i algorytmy planowania ruchu - laboratorium

Lab 2 - Struktury danych w planowaniu ruchu - wyświetlanie w RViz i dostęp z poziomu kodu

humble

1. Wprowadzenie

Celem zajęć jest poznanie mechanizmów subskrybowania danych z topików, które przechowują informację o modelu otoczenia robota. Testowana będzie mapa wysokościowa i mapa zajętości.

⚠️ Pamiętaj, aby w każdym nowym terminalu zanim rozpoczniesz pracę skonfigurować środowisko ROS komendą
source /opt/ros/humble/setup.bash lub source install/setup.bash

2. Mapy wysokościowe - elevation map (ROS 1 Noetic)

Przykładowe tworzenie i wyświetlanie mapy wysokościowej pokazane zostanie na przykładzie paczki: https://github.com/ANYbotics/elevation_mapping

Paczka ta nie została jeszcze w pełni zaimplementowana w ROS 2, dlatego tę część instrukcji (pkt. 2.) wykonać należy w ROS 1 (w wersji Noetic). W tym celu proszę skorzystać z przygotowanego obrazu w dockerze (obraz ma już zainstalowane wymagane pakiety ROSa).

Jeśli korzystasz z komputera w sali lab. 321, użyj komendy docker images, aby sprawdzić, czy na liście jest obraz o nazwie ros1_miapr. Jeśli nie ma takiego obrazu, pobierz plik tar z obrazem i go wczytaj:

mkdir -p ~/miapr && cd ~/miapr

pip install --upgrade --no-cache-dir gdown

test -f ros1_img.tar || python3 -m gdown "https://drive.google.com/uc?id=1xMOBmKESodcqaL6PYAT1zSdC5IGLToSP&confirm=t" 

docker load -i ros1_img.tar

Później uruchom kontener poleceniem:

xhost + && docker run -it \
    --env="DISPLAY" \
    --env="QT_X11_NO_MITSHM=1" \
    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
    --net=host \
    --privileged \
    --gpus=all \
    --name=ros1_miapr_lab2 \
    ros1_miapr:latest

Aby otworzyć kolejny terminal w dockerze, należy korzystać z polecenia:

docker exec -it ros1_miapr_lab2 bash

Poniższe komendy wykonuj w kontenerze. Pobierz biblioteki z repozytorium github:

cd /home/catkin_ws/src
git clone https://github.com/ANYbotics/elevation_mapping
git clone https://github.com/ANYbotics/kindr.git
git clone https://github.com/ANYbotics/kindr_ros.git
git clone https://github.com/ANYbotics/message_logger.git 
git clone https://github.com/ANYbotics/point_cloud_io.git

Następnie skompiluj pobraną paczkę i odśwież przestrzeń roboczą:

cd /home/catkin_ws
source /opt/ros/noetic/setup.bash
catkin_make
source devel/setup.bash

Uruchom symulację poleceniem i czekaj (uruchomienie symulacji po raz pierwszy może zająć kilka minut):

roslaunch elevation_mapping_demos turtlesim3_waffle_demo.launch

Powinien pojawić się robot turtlebot3 waffle w środowisku Gazebo. Aby sterować robotem z klawiatury, należy użyć modułu turtlebot3_teleop (w osobnym terminalu):

export TURTLEBOT3_MODEL=waffle
roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

Sterowanie robotem odbywa się za pomocą klawiszy ‘a’, ‘s’, ‘d’, ‘w’, ‘x’. Podczas poruszania się robota powinna budować się mapa wysokościowa terenu. Aktualizacja mapy może zająć około 60 s, dlatego należy poruszać się z niewielką prędkością.

🔨 Zadanie 2.1

Zmienić rozmiar mapy na większy np. 30x30 - należy ustawić parametry length_in_x, length_in_y otwierając odpowiedni plik komendą:

nano src/elevation_mapping/elevation_mapping_demos/config/robots/waffle_robot.yaml

🔨 Zadanie 2.2

Uruchomić ponownie symulację. Wyświetlić w RViz chmurę punktów z kamery RGB-D zamontowanej na robocie (topik /camera/depth/points).

🔨 Zadanie 2.3

Używając sterowania za pomocą klawiatury, zbudować mapę wysokościową pomieszczenia. Oczekiwany efekt w RViz wygląda następująco:

humble

🔨 Zadanie 2.4

Odwołując się do usługi /elevation_mapping/save_map, zapisać mapę poleceniem (nazwa topika powinna zostać pusta, zapis może trwać kilka minut):

⚠️ Kopiowanie polecenia z instrukcji może nie działać - zamiast tego napisz polecenie ręcznie, korzystając z przycisku Tab do podpowiadania składni LUB skorzystaj z narzędzia okienkowego:
rosrun rqt_service_caller rqt_service_caller

rosservice call /elevation_mapping/save_map "file_path: '/home/catkin_ws/elevation_map.bag'
topic_name: ''"

🔨 Zadanie 2.5

Zamknąć i uruchomić symulację ponownie. Odwołując się do usługi /elevation_mapping/load_map, wczytać zapisaną mapę poleceniem:

⚠️ Kopiowanie polecenia z instrukcji może nie działać - zamiast tego napisz polecenie ręcznie, korzystając z przycisku Tab do podpowiadania składni LUB skorzystaj z narzędzia okienkowego:
rosrun rqt_service_caller rqt_service_caller

rosservice call /elevation_mapping/load_map "file_path: '/home/catkin_ws/elevation_map.bag'
topic_name: ''"

Pozytywne wczytanie mapy powinno zakończyć się komunikatem: success: True

🔨 Zadanie 2.6

Zamknąć i uruchomić symulację ponownie. Odwołując się do usługi /elevation_mapping/load_map, wczytać zapisaną mapę całego pomieszczenia. Mapa (elevation_map.bag) znajduje się w katalogu /home/catkin_ws/src/example_maps/maps. Wcześniej trzeba pobrać repozytorium z przykładową mapą:

cd /home/catkin_ws/src
git clone https://github.com/dominikbelter/example_maps

3. Dostęp do mapy zajętości z poziomu węzła (node) napisanego w języku Python (ROS 2 Humble)

Aby ułatwić sobie pisanie kodu, warto wykorzystać zainstalowane na komputerze IDE - Visual Studio Code lub PyCharm. Aby z powodzeniem korzystać w IDE z funkcjonalności związanych z ROS’em, należy otworzyć terminal i wykonać komendy:

PyCharm

cd ~/ros2_ws
source install/setup.bash 
pycharm-community .

VS Code
```
cd ~/ros2_ws
source install/setup.bash 
code .
```
W VS Code zainstalować rozszerzenia (Extensions): Python i ROS.

3.1 Dostęp do komórek mapy zajętości

Na początku uruchomimy przykładową mapę zajętości tak samo jak na poprzednich zajęciach:

cd ~/ros2_ws/src
git clone --branch humble https://github.com/dominikbelter/example_maps

Skompiluj wszystkie paczki znajdujące się w przestrzeni roboczej:

cd ~/ros2_ws/
colcon build --symlink-install

Jeżeli kompilacja zakończyła się sukcesem, można uruchomić serwer, który udostępnia mapę zajętości:

ros2 run nav2_map_server map_server --ros-args --params-file src/example_maps/param/map_server_params.yaml
ros2 lifecycle set /map_server configure

Mapę można również wyświetlić w programie przeznaczonym do wizualizacji RViz:

ros2 run rviz2 rviz2

Wybierz odpowiedni topik, aby wyświetlić mapę zajętości jak na pierwszych zajęciach i aktywujemy map_server

ros2 lifecycle set /map_server activate

W kolejnym kroku utworzymy paczkę, której zadaniem będzie subskrybowanie mapy zapisanej w topiku /map. Użyjemy do tego polecenia:

cd ~/ros2_ws/src
ros2 pkg create map_manipulation --dependencies rclpy nav_msgs --build-type ament_python

Zanim zaczniemy pisać kod, sprawdzimy typ przechowywanej mapy:

ros2 topic info /map

Polecenie wyświetla typ danych dla topika i listę węzłów, które subskrybują i publikują te dane. Mapa zajętości jest typu nav_msgs/msg/OccupancyGrid. Ten typ zdefiniowany jest na stronie: https://docs.ros2.org/foxy/api/nav_msgs/msg/OccupancyGrid.html

Dane mapy przechowywane są w polu tablicy jednowymiarowej data. Prawdopodobieństwo zajętości zdefiniowane jest w przedziale [0,100], nieznana komórka ma wartość -1. W strukturze MapMetaData info przechowywane są informacje o liczbie wierszy, kolumn, położeniu mapy, oraz o wielkości komórek.

Przejdźmy teraz do katalogu ~/ros2_ws/src/map_manipulation/map_manipulation:

cd ~/ros2_ws/src/map_manipulation/map_manipulation

I utworzymy skrypt subskrybujący i publikujący dane:

touch publisher_member_function.py

Plik należy otworzyć w IDE. Napiszemy teraz program, który pobiera dane z mapy zajętości i publikuje je w topiku o nazwie /map_copy:

import rclpy
from rclpy.node import Node
from rclpy import qos
import copy
from nav_msgs.msg import OccupancyGrid

class MapSubscriber(Node):

    def __init__(self):
        super().__init__('map_subscriber')
        qos_profile = qos.QoSProfile(depth=10)
        qos_profile.durability = qos.DurabilityPolicy.TRANSIENT_LOCAL
        self.publisher_ = self.create_publisher(OccupancyGrid, 'map_copy', qos_profile)
        self.subscription = self.create_subscription(
            OccupancyGrid,
            'map',
            self.listener_callback,
            qos_profile)
        self.subscription  # prevent unused variable warning

    def listener_callback(self, msg):
        map_cpy = OccupancyGrid()
        map_cpy = copy.deepcopy(msg)
        self.get_logger().info('I heard a map')
        self.publisher_.publish(map_cpy)
        self.get_logger().info('Publishing map')

def main(args=None):
    rclpy.init(args=args)

    map_subscriber = MapSubscriber()
    rclpy.spin(map_subscriber)

    # Destroy the node explicitly (optional - otherwise it will be 
    # done automatically when the garbage collector destroys the node object)
    map_subscriber.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
   main()

Należy jeszcze skonfigurować tzw. entry point, czyli wskazać funkcję która jest wywoływana, gdy uruchamiamy węzeł. W tym celu w pliku setup.py dodać jedną linię w liście console_scripts:

entry_points={
        'console_scripts': [
            'map_manipulator = map_manipulation.publisher_member_function:main',
        ],
    },

Aby uruchomić węzeł, musimy go wcześniej skompilować (nie ma znaczenia, że piszemy w języku Python):

cd ~/ros2_ws
colcon build --symlink-install

Pamiętamy o aktualizacji środowiska:

source install/setup.bash

Następnie uruchamiamy napisany węzeł poleceniem:

ros2 run map_manipulation map_manipulator

Wyświetl uzyskaną mapę w RViz. Aby wymusić subskrybowanie i publikowanie danych, deaktywuj i aktywuj map_server:

ros2 lifecycle set /map_server deactivate
ros2 lifecycle set /map_server activate

lub zmień ustawienia topika /map_copy w Rvizie (Reliable, Transient Local):

humble

🔨 Zadanie 3.1.1

Napisz węzeł który pogrubi ściany oryginalnej mapy. Oczekiwany efekt znajduje się poniżej:

humble

Wskazówka:
Użyj funkcji, która sprawdza, czy w sąsiedztwie komórki o współrzędnych ‘col’ i ‘row’ znajduje się zajęta komórka:
def hasNeighbor(self, col, row, map):
   for i in range(-1, 2):
       for j in range(-1, 2):
           if (col + i >= 0 and col + i < map.info.width and row + j >= 0 and row + j < map.info.height):
               if (map.data[col + i + (row + j) * map.info.width]) > 50:
                   return True
   return False
Jeżeli w sąsiedztwie znajduje się zajęta komórka, to rozważaną komórkę zmodyfikuj jako zajętą.